/* 从上位机传下的指令都是字符串形式的，以0x0a结尾
    回复都是‘>’开头，然后是完整指令，然后是‘:’,然后是数据
    例如： 1.  down:   63 68 65 61 70 69 64 0a     cheapid
                up:     3e 63 68 65 61 70 69 64 3a 30 30 34 65 30 30 34 39 35 32 35 …   >cheapid:004e0049525
            2.  down:   70 62 63 6c 69 6e 6b 3f 0a  pbclink?
                up:     3e 70 62 63 6c 69 6e 6b 3f 3a 6e 6f 6c 69 6e 6b 0a  >pbclink?:nolink
            3.  down:   64 65 67 72 65 65 73 3d 32 35 32 30 0a  degrees=2520
                up: 3e 64 65 67 72 65 65 73 3d 32 35 32 30 3a 4f 4b 0a  >degrees=2520:OK

*/

#include "ffb_config.h"
#include <rthw.h>
#include "GQueue.h"
#include "rtthread.h"
#include <string.h>
#include <ctype.h>
#include <stdlib.h>
#include "eeprom.h"
#include "eeprom_addresses.h"
#include "FFBWheelMain.h"
#include "EffectsCalculator.h"
#include "USBCdc.h"
#include "motor.h"
#include "ver.h"
#include "canopen_motor.h"

#define CDC_CMD_COUNT       sizeof(cdcCmd)/sizeof(cdcCmd[0])

static Axis* axisConf = NULL;
static HidFFB* ffbConf = NULL;
static uint8_t ffbReplyBuf[CDC_PROCESS_BUF_SIZE];

static void degrees_Process         (uint8_t* srcStr,const struct strcmd* cmd);
static void power_Process           (uint8_t* srcStr,const struct strcmd* cmd);
static void fxratio_Process         (uint8_t* srcStr,const struct strcmd* cmd);
static void friction_Process        (uint8_t* srcStr,const struct strcmd* cmd);
static void damper_Process          (uint8_t* srcStr,const struct strcmd* cmd);
static void inertia_Process         (uint8_t* srcStr,const struct strcmd* cmd);
static void idlespring_Process      (uint8_t* srcStr,const struct strcmd* cmd);
static void endstop_Process         (uint8_t* srcStr,const struct strcmd* cmd);
static void maxspeed_Process        (uint8_t* srcStr,const struct strcmd* cmd);
static void invertx_Process         (uint8_t* srcStr,const struct strcmd* cmd);
static void cffilter_Process        (uint8_t* srcStr,const struct strcmd* cmd);
static void frfilter_Process        (uint8_t* srcStr,const struct strcmd* cmd);
static void dafilter_Process        (uint8_t* srcStr,const struct strcmd* cmd);
static void infilter_Process        (uint8_t* srcStr,const struct strcmd* cmd);
static void frgains_Process         (uint8_t* srcStr,const struct strcmd* cmd);
static void dagains_Process         (uint8_t* srcStr,const struct strcmd* cmd);
static void ingains_Process         (uint8_t* srcStr,const struct strcmd* cmd);
static void spgains_Process         (uint8_t* srcStr,const struct strcmd* cmd);
static void pfgains_Process         (uint8_t* srcStr,const struct strcmd* cmd);
static void swver_Process           (uint8_t* srcStr,const struct strcmd* cmd);
static void zeroenc_Process         (uint8_t* srcStr,const struct strcmd* cmd);
static void productid_Process       (uint8_t* srcStr,const struct strcmd* cmd);
static void reboot_Process          (uint8_t* srcStr,const struct strcmd* cmd);

const struct strcmd cdcCmd[] = {
    STRCMD(degrees),        // 方向盘可转角度  最大1080，实际可设置到1220
    STRCMD(power),          // 扭矩大小 %
    STRCMD(fxratio),        // 回馈强度
    STRCMD(friction),       // 自然摩擦
    STRCMD(damper),         // 自然阻尼
    STRCMD(inertia),        // 自然惯性
    STRCMD(idlespring),     // 回盘速度
    STRCMD(endstop),        // 超可转角度范围之后强度
    STRCMD(maxspeed),
    STRCMD(invertx),        // 反向   0：不反向；1：反向
    STRCMD(cffilter),       // constant force filter
    STRCMD(frfilter),       // friction filter
    STRCMD(dafilter),       // damper filter
    STRCMD(infilter),       // inertia filter
    STRCMD(frgains),        // friciton gain and scale
    STRCMD(dagains),        // damper gain and scale
    STRCMD(ingains),        // inertia gain and scale
    STRCMD(spgains),        // spring gain and scale
    STRCMD(pfgains),        // periodic force gain and scale
    STRCMD(swver),          // software version
    STRCMD(productid),
    STRCMD(zeroenc),        // 设置零点
    STRCMD(reboot),         // 重启设备
};


static size_t make_reply_header(uint8_t* srcStr,size_t len)
{
    if(srcStr == NULL || len > sizeof(ffbReplyBuf))
    {
        return 0;
    }
    size_t pos=0;
    /* make reply string*/
    ffbReplyBuf[pos++] = '>';
    //将原字符串拷过来
    memcpy(&ffbReplyBuf[pos],srcStr,len);
    pos += len;
    ffbReplyBuf[pos++] = ':';
    return pos;
}

void degrees_Process(uint8_t* srcStr,const struct strcmd* cmd)
{
    if(axisConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        sprintf((char*)&ffbReplyBuf[pos],"%d",axisConf->conf.degrees);
    }
    else if(srcStr[cmd->len] == '=')
    {
        axisConf->conf.degrees = atoi((const char*)&srcStr[cmd->len+1]);
        AxisSetDegrees(axisConf,axisConf->conf.degrees);
        EE_WriteVariable(axisConf->flashAddrs->addr_degrees,axisConf->conf.degrees);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));

}

void power_Process          (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(axisConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        sprintf((char*)&ffbReplyBuf[pos],"%d",axisConf->conf.power);
    }
    else if(srcStr[cmd->len] == '=')
    {
        axisConf->conf.power = atoi((const char*)&srcStr[cmd->len+1]);
        AxisSetPower(axisConf,axisConf->conf.power);
        //加入OK
        EE_WriteVariable(axisConf->flashAddrs->addr_power,axisConf->conf.power);
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));


}
void fxratio_Process        (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(axisConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        sprintf((char*)&ffbReplyBuf[pos],"%d",axisConf->conf.fxratio);
    }
    else if(srcStr[cmd->len] == '=')
    {
        axisConf->conf.fxratio = atoi((const char*)&srcStr[cmd->len+1]);
        AxisSetFxRatio(axisConf,axisConf->conf.fxratio);
        EE_WriteVariable(axisConf->flashAddrs->addr_fxratio,axisConf->conf.fxratio);
        // 加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    // 加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
}
void friction_Process       (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(axisConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        sprintf((char*)&ffbReplyBuf[pos],"%d",axisConf->conf.friction);
    }
    else if(srcStr[cmd->len] == '=')
    {
        axisConf->conf.friction = atoi((const char*)&srcStr[cmd->len+1]);

        AxisSetFriction(axisConf,axisConf->conf.friction);

        EE_WriteVariable(axisConf->flashAddrs->addr_friction,axisConf->conf.friction);

        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
}
void damper_Process     (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(axisConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        sprintf((char*)&ffbReplyBuf[pos],"%d",axisConf->conf.damper);
    }
    else if(srcStr[cmd->len] == '=')
    {
        axisConf->conf.damper = atoi((const char*)&srcStr[cmd->len+1]);
        AxisSetDamperStrength(axisConf,axisConf->conf.damper);
        EE_WriteVariable(axisConf->flashAddrs->addr_damper,axisConf->conf.damper);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
}
void inertia_Process        (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(axisConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        sprintf((char*)&ffbReplyBuf[pos],"%d",axisConf->conf.inertia);
    }
    else if(srcStr[cmd->len] == '=')
    {
        axisConf->conf.inertia = atoi((const char*)&srcStr[cmd->len+1]);
        CLIP(axisConf->conf.inertia,0,100);
        EE_WriteVariable(axisConf->flashAddrs->addr_inertia,axisConf->conf.inertia);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));

}
void idlespring_Process     (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(axisConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        sprintf((char*)&ffbReplyBuf[pos],"%d",axisConf->conf.idlespring);
    }
    else if(srcStr[cmd->len] == '=')
    {
        axisConf->conf.idlespring = atoi((const char*)&srcStr[cmd->len+1]);
        CLIP(axisConf->conf.idlespring,0,100);

        AxisSetIdleSpringStrength(axisConf,axisConf->conf.idlespring);
        EE_WriteVariable(axisConf->flashAddrs->addr_idlespring,axisConf->conf.idlespring);

        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));

}

static void endstop_Process     (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(axisConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        sprintf((char*)&ffbReplyBuf[pos],"%d",axisConf->conf.endstop);
    }
    else if(srcStr[cmd->len] == '=')
    {
        axisConf->conf.endstop = atoi((const char*)&srcStr[cmd->len+1]);
        CLIP(axisConf->conf.endstop,0,100);
        AxisSetEndStopStrength(axisConf,axisConf->conf.endstop);
        EE_WriteVariable(axisConf->flashAddrs->addr_endstop,axisConf->conf.endstop);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
}

void maxspeed_Process       (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(axisConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        sprintf((char*)&ffbReplyBuf[pos],"%d",axisConf->conf.maxSpeedDegS);
    }
    else if(srcStr[cmd->len] == '=')
    {
        axisConf->conf.maxSpeedDegS = atoi((const char*)&srcStr[cmd->len+1]);
        CLIP(axisConf->conf.maxSpeedDegS,60,600);
        AxisSetMaxSpeed(axisConf,axisConf->conf.maxSpeedDegS);
        EE_WriteVariable(axisConf->flashAddrs->addr_maxSpeed,axisConf->conf.maxSpeedDegS);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
}

void invertx_Process        (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(axisConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        sprintf((char*)&ffbReplyBuf[pos],"%d",axisConf->conf.invertx);
    }
    else if(srcStr[cmd->len] == '=')
    {
        axisConf->conf.invertx = atoi((const char*)&srcStr[cmd->len+1]);
        AxisSetInvertx(axisConf,axisConf->conf.invertx);
        EE_WriteVariable(axisConf->flashAddrs->addr_invertx,axisConf->conf.invertx);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));

}

static void cffilter_Process        (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(ffbConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    uint16_t f = 0;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        EC_Filter_Read(ffbConf->effects_calc,FFB_EFFECT_CONSTANT,&f);
        sprintf((char*)&ffbReplyBuf[pos],"%d",f);
    }
    else if(srcStr[cmd->len] == '=')
    {
        f = atoi((const char*)&srcStr[cmd->len+1]);
        EC_Filter_Write(ffbConf->effects_calc,FFB_EFFECT_CONSTANT,f);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
}
static void frfilter_Process        (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(ffbConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    uint16_t f = 0;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        EC_Filter_Read(ffbConf->effects_calc,FFB_EFFECT_FRICTION,&f);
        sprintf((char*)&ffbReplyBuf[pos],"%d",f);
    }
    else if(srcStr[cmd->len] == '=')
    {
        f = atoi((const char*)&srcStr[cmd->len+1]);
        EC_Filter_Write(ffbConf->effects_calc,FFB_EFFECT_FRICTION,f);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
}
static void dafilter_Process        (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(ffbConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    uint16_t f = 0;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        EC_Filter_Read(ffbConf->effects_calc,FFB_EFFECT_DAMPER,&f);
        sprintf((char*)&ffbReplyBuf[pos],"%d",f);
    }
    else if(srcStr[cmd->len] == '=')
    {
        f = atoi((const char*)&srcStr[cmd->len+1]);
        EC_Filter_Write(ffbConf->effects_calc,FFB_EFFECT_DAMPER,f);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
}
static void infilter_Process        (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(ffbConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    uint16_t f = 0;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        EC_Filter_Read(ffbConf->effects_calc,FFB_EFFECT_INERTIA,&f);
        sprintf((char*)&ffbReplyBuf[pos],"%d",f);
    }
    else if(srcStr[cmd->len] == '=')
    {
        f = atoi((const char*)&srcStr[cmd->len+1]);
        EC_Filter_Write(ffbConf->effects_calc,FFB_EFFECT_INERTIA,f);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
}
static void frgains_Process         (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(ffbConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    uint16_t gs = 0;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        EC_Gain_Scaler_Read(ffbConf->effects_calc,FFB_EFFECT_FRICTION,&gs);
        sprintf((char*)&ffbReplyBuf[pos],"%d",gs);
    }
    else if(srcStr[cmd->len] == '=')
    {
        gs = atoi((const char*)&srcStr[cmd->len+1]);
        EC_Gain_Scaler_Write(ffbConf->effects_calc,FFB_EFFECT_FRICTION,gs);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
}
static void dagains_Process         (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(ffbConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    uint16_t gs = 0;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        EC_Gain_Scaler_Read(ffbConf->effects_calc,FFB_EFFECT_DAMPER,&gs);
        sprintf((char*)&ffbReplyBuf[pos],"%d",gs);
    }
    else if(srcStr[cmd->len] == '=')
    {
        gs = atoi((const char*)&srcStr[cmd->len+1]);
        EC_Gain_Scaler_Write(ffbConf->effects_calc,FFB_EFFECT_DAMPER,gs);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
}
static void ingains_Process         (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(ffbConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    uint16_t gs = 0;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        EC_Gain_Scaler_Read(ffbConf->effects_calc,FFB_EFFECT_INERTIA,&gs);
        sprintf((char*)&ffbReplyBuf[pos],"%d",gs);
    }
    else if(srcStr[cmd->len] == '=')
    {
        gs = atoi((const char*)&srcStr[cmd->len+1]);
        EC_Gain_Scaler_Write(ffbConf->effects_calc,FFB_EFFECT_INERTIA,gs);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
}
static void spgains_Process         (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(ffbConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    uint16_t gs = 0;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        EC_Gain_Scaler_Read(ffbConf->effects_calc,FFB_EFFECT_SPRING,&gs);
        sprintf((char*)&ffbReplyBuf[pos],"%d",gs);
    }
    else if(srcStr[cmd->len] == '=')
    {
        gs = atoi((const char*)&srcStr[cmd->len+1]);
        EC_Gain_Scaler_Write(ffbConf->effects_calc,FFB_EFFECT_SPRING,gs);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
}

static void pfgains_Process         (uint8_t* srcStr,const struct strcmd* cmd)
{
    if(ffbConf == NULL)
    {
        return;
    }
    size_t pos=0;
    size_t originalStrLen;
    uint16_t gs = 0;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        EC_Gain_Scaler_Read(ffbConf->effects_calc,FFB_EFFECT_SINE,&gs);
        sprintf((char*)&ffbReplyBuf[pos],"%d",gs);
    }
    else if(srcStr[cmd->len] == '=')
    {
        gs = atoi((const char*)&srcStr[cmd->len+1]);
        EC_Gain_Scaler_Write(ffbConf->effects_calc,FFB_EFFECT_SINE,gs);
        //加入OK
        ffbReplyBuf[pos++] = 'O';
        ffbReplyBuf[pos++] = 'K';
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
}

void swver_Process          (uint8_t* srcStr,const struct strcmd* cmd)
{
    size_t pos=0;
    size_t originalStrLen;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        memcpy(&ffbReplyBuf[pos],SOFTVER,strlen(SOFTVER));
    }
    else        //只能查询，不能设置
    {
        return;
    }
    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));

}
//INTEGER32 Position_actual_value = 0x0;
extern ffb_wheel_t ffbWheel;
void zeroenc_Process        (uint8_t* srcStr,const struct strcmd* cmd)
{
    size_t pos=0;
    size_t originalStrLen;
    uint16_t flashStatus;
    uint16_t flashTryCount=0;
    canopen_motor_t* motor = NULL;
    int32_t curPos = 0;
    uint16_t posH = 0;
    uint16_t posL = 0;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    memcpy(&ffbReplyBuf[pos],"OK",strlen("OK"));

    //回零处理
    motor = Canopen_Motor_Get(1);
    curPos = motor->actualPos;
    posH = (curPos>>16)&0xFFFF;
    posL = curPos&0xFFFF;
    Axis* axis = &ffbWheel.axisX;
    AxisSetHome(axis,curPos);
//    int level = rt_hw_interrupt_disable();
    flashStatus = EE_WriteVariable(axisConf->flashAddrs->addr_homePosL16,posL);
    while(flashStatus != HAL_OK && flashTryCount < FLASH_TRY_TIMES){
        flashStatus = EE_WriteVariable(axisConf->flashAddrs->addr_homePosL16,posL);
        flashTryCount++;
    }
    flashTryCount = 0;
    flashStatus = EE_WriteVariable(axisConf->flashAddrs->addr_homePosH16,posH);
    while(flashStatus != HAL_OK && flashTryCount < FLASH_TRY_TIMES){
            flashStatus = EE_WriteVariable(axisConf->flashAddrs->addr_homePosH16,posH);
            flashTryCount++;
        }

//    rt_hw_interrupt_enable(level);
    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));

}

void reboot_Process         (uint8_t* srcStr,const struct strcmd* cmd)
{
    size_t pos=0;
    size_t originalStrLen;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    memcpy(&ffbReplyBuf[pos],"OK",strlen("OK"));

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));
    Canopen_Motor_Stop_Node();
    rt_thread_delay(500);
    __set_PRIMASK(1);
    NVIC_SystemReset();

}

extern uint8_t usbSerial[25];
void productid_Process      (uint8_t* srcStr,const struct strcmd* cmd)
{
    size_t pos=0;
    size_t originalStrLen;
    originalStrLen = strlen((const char*)srcStr);
    pos = make_reply_header(srcStr,originalStrLen);
    if(srcStr[cmd->len] == '?')
    {
        memcpy(&ffbReplyBuf[pos],usbSerial,strlen((const char*)usbSerial));
    }
    else        //只能查询，不能设置
    {
        return;
    }

    //加入结束符0x0a
    ffbReplyBuf[strlen((const char*)ffbReplyBuf)] = 0x0a;
    CDC_Send(ffbReplyBuf,strlen((const char*)ffbReplyBuf));

}

void CDC_Configure_Init(struct Axis* config,HidFFB* ffb)
{
    axisConf = config;
    ffbConf = ffb;
}

void FFB_Config_Process(uint8_t* str,int strLen)
{

    uint8_t strStart = 0;
    uint8_t found;
    uint8_t* pStr = str;
    uint8_t* cmdBuf;
    int srcStrLen = 0;
    // 1.舍弃前面的控制字符
    for(int i = 0; i < strLen; i++)
    {
        if(!iscntrl(pStr[i]))
        {
            strStart = i;
            break;
        }
    }
    found = 0;
    // 2.结束字符串处理
    for(int i= strStart; i < strLen; i++)
    {
        //字符串结束
        if(pStr[i] == 0x0a)
        {
            pStr[i] = 0;
            found = 1;
        }

    }

    // 3.从缓冲区中查找指令字符串并处理
    if(found)
    {
        srcStrLen = strlen((const char*)&pStr[strStart]);
        for(int i = 0; i < CDC_CMD_COUNT; i++)
        {
            if(srcStrLen >= cdcCmd[i].len)
            {
                //获取匹配字符串的指针
                cmdBuf = (uint8_t*)strstr((char*)&pStr[strStart],(const char*)cdcCmd[i].str);
                if(cmdBuf != NULL && cdcCmd[i].process != NULL)
                {
                    //清空回复数组，为回复做准备
                    memset(ffbReplyBuf,0,sizeof(ffbReplyBuf));
                    cdcCmd[i].process(cmdBuf,&cdcCmd[i]);
                    break;
                }
            }
        }
    }
}


